#include <Servo.h>
#include <Arduino.h>
#include <Wire.h>
#include <U8g2lib.h>

U8G2_SSD1306_128X64_NONAME_F_HW_I2C U8g2(U8G2_R0, U8X8_PIN_NONE);

int servoPin = 9;                // servo pin
int val;                         // angle set
int ledPinMatriex[] = { 5, 6 };  // led pin

Servo servo;  // get a servo object

const int TrigPin = 11;
const int EchoPin = 12;
float distance = 0;

// init pins
void setup() {
  // servo init
  servo.attach(servoPin, 500, 2480);  // reset
  // serial init
  Serial.begin(9600);
  // led init
  for (int i = 0; i < 2; i++) {
    pinMode(ledPinMatriex[i], OUTPUT);
    analogWrite(ledPinMatriex[i], 0);
  }
  pinMode(TrigPin, OUTPUT);
  pinMode(EchoPin, INPUT);
  Serial.println("Ultrasonic sensor:");
  U8g2.begin();
}

// control
void control(char comd) {
  val = int(comd) - int('0');   // transform rad angle
  int angle = val * (180 / 9);  // final angle
  // limit the angle range
  if (angle >= 180)
    angle = 180;
  if (angle <= 0)
    angle = 0;
  int lightRange = 0;
  for (int i = 0; i < angle; i++) {
    servo.write(i);
    // light range
    lightRange = i / angle * 100;
    analogWrite(ledPinMatriex[0], lightRange);
    analogWrite(ledPinMatriex[1], 100 - lightRange);
    delay(10);
  }
  for (int i = angle; i > 0; i--) {
    servo.write(i);
    lightRange = i / angle * 100;
    analogWrite(ledPinMatriex[1], lightRange);
    analogWrite(ledPinMatriex[0], 100 - lightRange);
    delay(10);
  }
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(100);
  digitalWrite(TrigPin, LOW);
  distance = pulseIn(EchoPin, HIGH) / (1000 / 340 * 2);
  Serial.print("distance: ");
  Serial.print(distance);
  Serial.print("cm");
  Serial.println();
  delay(500);
}

// loop function
void loop() {
  distance = 0;
  while (Serial.available()) {
    char comd = Serial.read();  // read data
    switch (comd) {
      case '1':
        {
          control(comd);
        }
        break;
      case '2':
        {
          control(comd);
        }
        break;
      case '3':
        {
          control(comd);
        }
        break;
      case '4':
        {
          control(comd);
        }
        break;
      case '5':
        {
          control(comd);
        }
        break;
      case '6':
        {
          control(comd);
        }
        break;
      case '7':
        {
          control(comd);
        }
        break;
      case '8':
        {
          control(comd);
        }
        break;
      case '9':
        {
          control(comd);
        }
        break;
      default:
        analogWrite(ledPinMatriex[1], 0);
        analogWrite(ledPinMatriex[0], 0);
    }
  }
  analogWrite(ledPinMatriex[1], 0);
  analogWrite(ledPinMatriex[0], 0);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(100);
  digitalWrite(TrigPin, LOW);
  distance = pulseIn(EchoPin, HIGH) / (1000 / 340 * 2);
  Serial.print("distance: ");
  Serial.print(distance);
  Serial.print("cm");
  Serial.println();
  delay(500);
}
